/*
修改接口函数（角度获取，速度获取，pid输出）的具体实现，即可使用
*/

#include "User.h"
#include "encode_ci.h"
#include "usart.h"
#include <stdint.h>


/* 接口函数，移植时只需修改它们即可 */
//接口函数1：获取当前角度
float foc_get_angle(void) {
  float now_elect_angle = normal_to_elect(AS5600_RawAngleToRadians(encoders[0].angle_now));
  return now_elect_angle;
}

//接口函数2：获取当前速度
float foc_get_speed(void) {
  static int angle_total_pre;
  float speed = encode_get_speed(encoders[0].angle_total_now, angle_total_pre);
  angle_total_pre = encoders[0].angle_total_now;
  usart_printf("%.2f,%.2f\n", speed,pids[0].target);
  return speed;
}

// 接口函数3：pid输出
float foc_pid_output(float now_value) {
  float output = pid_value_update(&pids[0], now_value);
  return output;
}

// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {
  float dc_a = 0.0f, dc_b = 0.0f, dc_c = 0.0f;
  // 计算占空比
  dc_a = _constrain(Ua / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
  dc_b = _constrain(Ub / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
  dc_c = _constrain(Uc / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);

  // 设置PWM占空比
  __HAL_TIM_SET_COMPARE(&PWM_TIM_HANDLE, PWM_TIM_CHANNEL_A, dc_a * PWM_PERIOD);
  __HAL_TIM_SET_COMPARE(&PWM_TIM_HANDLE, PWM_TIM_CHANNEL_B, dc_b * PWM_PERIOD);
  __HAL_TIM_SET_COMPARE(&PWM_TIM_HANDLE, PWM_TIM_CHANNEL_C, dc_c * PWM_PERIOD);
}

void setPhaseVoltage(float Uq, float angle_el) {
  float Ualpha = 0.0f, Ubeta = 0.0f;
  float Ua = 0.0f, Ub = 0.0f, Uc = 0.0f;
   
  angle_el = _normalizeAngle(angle_el);


  // 帕克逆变换
  Ualpha = -Uq * sinf(angle_el);
  Ubeta = Uq * cosf(angle_el);

  // 克拉克逆变换
  Ua = Ualpha + VOLTAGE_POWER_SUPPLY / 2;
  Ub = (sqrtf(3) * Ubeta - Ualpha) / 2 + VOLTAGE_POWER_SUPPLY / 2;
  Uc = (-Ualpha - sqrtf(3) * Ubeta) / 2 + VOLTAGE_POWER_SUPPLY / 2;
  // usart_printf("%.2f,%.2f,%.2f\n", Ua, Ub, Uc);


  setPwm(Ua, Ub, Uc);
}

// 开环速度函数
float velocityOpenloop(float target_velocity) {
  // static uint32_t open_loop_timestamp = 0;
  // uint32_t now_us = HAL_GetTick() * 1000; // 转换为微秒近似值
  // float Ts = (now_us - open_loop_timestamp) * 1e-6f;
  

  // if (Ts <= 0 || Ts > 0.5f)
  //   Ts = 1e-3f;

  static float shaft_angle = 0.0f;
  //生成目标角度，并限制在【0，6.28】
  float a = fmodf(shaft_angle - target_velocity * encode_period_time, 2 * PI);
  shaft_angle = (a >= 0) ? a : (a + 2 * PI);
  uint8_t Uq = 6;
  setPhaseVoltage(Uq, _electricalAngle(shaft_angle, POLE_PAIR));

  // open_loop_timestamp = now_us;
  return Uq;
}

void foc_task(void) {

  // 尝试闭环跑并获取速度
  float now_elect_angle = foc_get_angle();
  float speed = foc_get_speed();
  float output = foc_pid_output(speed);
  setPhaseVoltage(output, (now_elect_angle));
}
